#include "dkglObject.h"
#include "dkglQuaternion.h"
#include "dkglMatrix4.h"
#include <typeinfo>

using namespace std;

namespace DKGL
{

void Axes::render()
{
	GLUquadricObj *quadric;
	glPushMatrix();
	
	glPushAttrib(GL_LIGHTING_BIT | GL_POLYGON_BIT);
	glDisable(GL_LIGHTING);
	glPolygonMode( GL_FRONT,GL_FILL);
	
	quadric = gluNewQuadric();

	glColor3ub(0xFF, 0x0, 0x0);
	gluSphere(quadric,mRadius*1.3f,8,8);
/////////////////////////////////////
	glColor3ub(0x0, 0x0, 0xFF);
	gluCylinder(quadric,mRadius, mRadius ,mLength,8,8);

	glTranslatef(0.0f, 0.0f, mLength);
	gluCylinder(quadric,mRadius*2.0f,mRadius*0.01f,mLength/3,8,8);
	glTranslatef(0.0f, 0.0f, -mLength);
/////////////////////////////////////
	glRotatef(-90.0f, 1.0f, 0.0f, 0.0f);
	glColor3ub(0x0, 0xFF, 0x0);
	gluCylinder(quadric,mRadius, mRadius ,mLength,8,8);

	glTranslatef(0.0f, 0.0f, mLength);
	gluCylinder(quadric,mRadius*2.0f,mRadius*0.01f,mLength/3,8,8);
	glTranslatef(0.0f, 0.0f, -mLength);
//////////////////////////////////////
	glRotatef(90.0f, 0.0f, 1.0f, 0.0f);
	glColor3ub(0xFF, 0x0, 0x0);
	gluCylinder(quadric,mRadius, mRadius ,mLength,8,8);

	glTranslatef(0.0f, 0.0f, mLength);
	gluCylinder(quadric,mRadius*2.0f,mRadius*0.01f,mLength/3,8,8);
	glTranslatef(0.0f, 0.0f, -mLength);

	gluDeleteQuadric(quadric);
	glPopMatrix();
	glPopAttrib();
}

BaseObject::BaseObject(const std::string& name) : mName(name)
{
	//Log::dump(name + " ctor");
	_initParam();
}

void BaseObject::_initParam()
{
	mParent = NULL;
	mRenderProfileName.assign("GLOBAL_PROFILE");
	mShow = true;
	mShowAxes = false;
	mShowBBox = false;
	mShowNormal = false;
	mNeedCalculateBBox = true;
	mNeedCalculateNormal = true;
	mEnableLighting = false;
	mRenderMode = RM_POLYGON;
	//mEnableColorTracking = true;
	SetIdentityMatrix(mLocalMatrix);
}

BaseObject::~BaseObject()
{
	Log::dump(string("dtor of ") + mName);
	if(mParent)
		mParent->popChild(mName);

	std::list<BaseObject*>::iterator it;
	for(it = mChildrenList.begin(); it!= mChildrenList.end(); it++)
	{
		(*it)->assignParent(NULL);

		if((*it)->isOnHeap())
		{
			Log::dump("This 3d object is on the heap");
			HeapTracked::operator delete(*it);
		}
	}
	mChildrenList.clear();
}

void BaseObject::getParentTransformation(float *m) const 
{
	glMatrixMode(GL_MODELVIEW);
	if(mParent!=NULL)
	{
		mParent->getParentTransformation(m);
		glPushMatrix();
		glLoadMatrixf(m);
		glMultMatrixf(mLocalMatrix);
		glGetFloatv(GL_MODELVIEW_MATRIX, m);
		glPopMatrix();
	}
	else
	{
		CopyMatrix(m, mLocalMatrix);
	}
}

void BaseObject::getParentRotationPart(float *m) const 
{
	float _m[16];
	//glMatrixMode(GL_MODELVIEW);
	if(mParent!=NULL)
	{
		mParent->getParentRotationPart(_m);
		m[0] = _m[0]*mLocalMatrix[0] + _m[4]*mLocalMatrix[1] + _m[8]*mLocalMatrix[2];
		m[1] = _m[1]*mLocalMatrix[0] + _m[5]*mLocalMatrix[1] + _m[9]*mLocalMatrix[2];
		m[2] = _m[2]*mLocalMatrix[0] + _m[6]*mLocalMatrix[1] + _m[10]*mLocalMatrix[2];

		m[4] = _m[0]*mLocalMatrix[4] + _m[4]*mLocalMatrix[5] + _m[8]*mLocalMatrix[6];
		m[5] = _m[1]*mLocalMatrix[4] + _m[5]*mLocalMatrix[5] + _m[9]*mLocalMatrix[6];
		m[6] = _m[2]*mLocalMatrix[4] + _m[6]*mLocalMatrix[5] + _m[10]*mLocalMatrix[6];

		m[8] = _m[0]*mLocalMatrix[8] + _m[4]*mLocalMatrix[9] + _m[8]*mLocalMatrix[10];
		m[9] = _m[1]*mLocalMatrix[8] + _m[5]*mLocalMatrix[9] + _m[9]*mLocalMatrix[10];
		m[10] = _m[2]*mLocalMatrix[8] + _m[6]*mLocalMatrix[9] + _m[10]*mLocalMatrix[10];
	}
	else
	{
		m[0] = mLocalMatrix[0];
		m[1] = mLocalMatrix[1];
		m[2] = mLocalMatrix[2];

		m[4] = mLocalMatrix[4];
		m[5] = mLocalMatrix[5];
		m[6] = mLocalMatrix[6];

		m[8] = mLocalMatrix[8];
		m[9] = mLocalMatrix[9];
		m[10] = mLocalMatrix[10];
	}
}

void BaseObject::setPosition(const Vector3& pos)
{
	mLocalMatrix[12] = pos.x;
	mLocalMatrix[13] = pos.y;
	mLocalMatrix[14] = pos.z;
}
	
void BaseObject::setPosition(float x, float y, float z)
{
	mLocalMatrix[12] = x;
	mLocalMatrix[13] = y;
	mLocalMatrix[14] = z;
}
	
Vector3 BaseObject::getPosition() const
{
	return Vector3(mLocalMatrix[12], mLocalMatrix[13], mLocalMatrix[14]);
}

Vector3 BaseObject::getWorldPosition() const
{
	float m[16];
	getParentTransformation(m);

	return Vector3(m[12], m[13], m[14]);
}

Vector3 BaseObject::localAxisInParentSpace(const Vector3& axis_in_local) const
{
	Vector3 ret;

	ret.x = mLocalMatrix[0]*axis_in_local.x + mLocalMatrix[4]*axis_in_local.y + mLocalMatrix[8]*axis_in_local.z;
	ret.y = mLocalMatrix[1]*axis_in_local.x + mLocalMatrix[5]*axis_in_local.y + mLocalMatrix[9]*axis_in_local.z;
	ret.z = mLocalMatrix[2]*axis_in_local.x + mLocalMatrix[6]*axis_in_local.y + mLocalMatrix[10]*axis_in_local.z;

	return  ret;
}

Vector3 BaseObject::localAxisInWorldSpace(const Vector3& axis_in_local) const
{
	float m[16];
	Vector3 ret;

	getParentRotationPart(m);

	ret.x = m[0]*axis_in_local.x + m[4]*axis_in_local.y + m[8]*axis_in_local.z;
	ret.y = m[1]*axis_in_local.x + m[5]*axis_in_local.y + m[9]*axis_in_local.z;
	ret.z = m[2]*axis_in_local.x + m[6]*axis_in_local.y + m[10]*axis_in_local.z;

	return ret;
}

void BaseObject::pushChild(BaseObject* child)
{
	mChildrenList.push_back(child);
	child->assignParent(this);
	//g_Log.dump(mName + "\" push child \"" + child->getName() + "\"");
}

BaseObject* BaseObject::popChild(const std::string& name)
{
	std::list<BaseObject*>::iterator i;
	BaseObject* ret;

	for(i=mChildrenList.begin();i!=mChildrenList.end();)
	{
		if((*i)->getName()==name)
		{
			Log::dump(string("Object ") + "\"" + mName + " popChild " + name);
			(*i)->assignParent(NULL);
			ret = (*i);
			i = mChildrenList.erase(i);
			return ret;
		}
		else
			i++;
	}
	return NULL;
}

void BaseObject::freeSelf()
{
	if(mParent==NULL)
		return;

	BaseObject* p = mParent;
	p->popChild(mName);
}

BaseObject* BaseObject::getChild(int index)
{
	list<BaseObject*>::iterator it;
	int i;

	for(i=0,it=mChildrenList.begin();it!=mChildrenList.end();it++, i++)
	{
		if( i==index )
			return *it;
	}

	return NULL;
}

BaseObject* BaseObject::getChild(const std::string& name)
{
	list<BaseObject*>::iterator it;

	for(it=mChildrenList.begin();it!=mChildrenList.end();it++)
	{
		if((*it)->mName==name)
			return *it;
	}

	return NULL;
}


#if 0
void BaseObject::rotate(float degree, const Vector3& axis,TRANS_SPACE space)
{
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();

	float _x, _y, _z;
	Quaternion	q, q0;
	Matrix4		m4x4;
	Matrix3		m3x3;

	float tmp[16];
	memcpy(tmp, mLocalMatrix, sizeof(float) * 16);

	if(space==LOCAL)
	{
		_x = mLocalMatrix[12]; _y = mLocalMatrix[13]; _z = mLocalMatrix[14];
		mLocalMatrix[12] = mLocalMatrix[13] = mLocalMatrix[14] = 0.0f;

		glLoadMatrixf( mLocalMatrix);
		glTranslatef( mLocalMatrix[12], mLocalMatrix[13], mLocalMatrix[14]);
		glRotatef(degree, axis.x, axis.y, axis.z);
		glTranslatef( -mLocalMatrix[12], -mLocalMatrix[13], -mLocalMatrix[14]);
		glGetFloatv(GL_MODELVIEW_MATRIX, mLocalMatrix);

		m4x4.init(mLocalMatrix);
		m4x4.extract3x3Matrix(m3x3);
		q0.FromRotationMatrix(m3x3);
		q.FromAngleAxis(Radian(Degree(degree)), axis);

		q = q * q0;
		q.ToRotationMatrix(m3x3);

		Matrix4(m3x3).load(mLocalMatrix);

		mLocalMatrix[12] = _x;
		mLocalMatrix[13] = _y;
		mLocalMatrix[14] = _z;
	}
	else if(space==GLOBAL)
	{
		glLoadIdentity();
		glRotatef(degree, axis.x, axis.y, axis.z);
		glMultMatrixf( mLocalMatrix);
		glGetFloatv(GL_MODELVIEW_MATRIX, mLocalMatrix);

		q.FromAngleAxis(Radian(Degree(degree)), axis);

		q.ToRotationMatrix(m3x3);

		m4x4.init(mLocalMatrix);
		m4x4 = Matrix4(m3x3) * m4x4;
		m4x4.load(mLocalMatrix);
	}
	glPopMatrix();
}
#else
void BaseObject::rotate(float degree, const Vector3& axis,TRANS_SPACE space)
{
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();

	if(space==LOCAL)
	{
		float _x, _y, _z;

		_x = mLocalMatrix[12]; _y = mLocalMatrix[13]; _z = mLocalMatrix[14];
		//mLocalMatrix[12] = mLocalMatrix[13] = mLocalMatrix[14] = 0.0f;

		glLoadMatrixf( mLocalMatrix);
		//glTranslatef( mLocalMatrix[12], mLocalMatrix[13], mLocalMatrix[14]);
		glRotatef(degree, axis.x, axis.y, axis.z);
		//glTranslatef( -mLocalMatrix[12], -mLocalMatrix[13], -mLocalMatrix[14]);
		glGetFloatv(GL_MODELVIEW_MATRIX, mLocalMatrix);

		mLocalMatrix[12] = _x;
		mLocalMatrix[13] = _y;
		mLocalMatrix[14] = _z;
	}
	else if(space==GLOBAL)
	{
		glLoadIdentity();
		glRotatef(degree, axis.x, axis.y, axis.z);
		glMultMatrixf( mLocalMatrix);
		glGetFloatv(GL_MODELVIEW_MATRIX, mLocalMatrix);
	}
	glPopMatrix();
}
#endif

void BaseObject::translate(const Vector3& axis, TRANS_SPACE space)
{
	float _x, _y, _z;
	if(space==LOCAL)
	{	
		_x = mLocalMatrix[0]*axis.x + mLocalMatrix[4]*axis.y + mLocalMatrix[8]*axis.z;
		_y = mLocalMatrix[1]*axis.x + mLocalMatrix[5]*axis.y + mLocalMatrix[9]*axis.z;
		_z = mLocalMatrix[2]*axis.x + mLocalMatrix[6]*axis.y + mLocalMatrix[10]*axis.z;

		mLocalMatrix[12] += _x;
		mLocalMatrix[13] += _y;
		mLocalMatrix[14] += _z;
	}
	else if( space == GLOBAL )
	{
		mLocalMatrix[12] += axis.x;
		mLocalMatrix[13] += axis.y;
		mLocalMatrix[14] += axis.z;
	}
}

void BaseObject::translate(float x, float y, float z, TRANS_SPACE space)
{
	float _x, _y, _z;

	if(space==LOCAL)
	{	
		_x = mLocalMatrix[0]*x + mLocalMatrix[4]*y + mLocalMatrix[8]*z;
		_y = mLocalMatrix[1]*x + mLocalMatrix[5]*y + mLocalMatrix[9]*z;
		_z = mLocalMatrix[2]*x + mLocalMatrix[6]*y + mLocalMatrix[10]*z;

		mLocalMatrix[12] += _x;
		mLocalMatrix[13] += _y;
		mLocalMatrix[14] += _z;
	}
	else if( space == GLOBAL )
	{
		mLocalMatrix[12] += x;
		mLocalMatrix[13] += y;
		mLocalMatrix[14] += z;
	}
}

void BaseObject::scale(const Vector3& s)
{
	//mScaleList.push_back(s);
}

void BaseObject::scale(float x, float y, float z)
{
	//mScaleList.push_back(Vector3(x, y, z));
}

void BaseObject::moveForward(float d)
{
	translate( Vector3(0, 0, -d), LOCAL);
	//mTransList.push_back(Vector3(0, 0, -d));
}

void BaseObject::moveRight(float d)
{
	translate( Vector3(d, 0, 0), LOCAL);
}

void BaseObject::moveUp(float d)
{
	translate( Vector3(0, d, 0), LOCAL);
}

void BaseObject::roll(float degree)
{
	rotate(degree, Vector3::UNIT_Z, LOCAL);
}


void BaseObject::yaw(float degree)
{
	rotate(degree, Vector3::UNIT_Y, LOCAL);
}

void BaseObject::pitch(float degree)
{
	rotate(degree, Vector3::UNIT_X, LOCAL);
}

void BaseObject::_transformation()
{
	glMultMatrixf(mLocalMatrix);

	if(mShowAxes) // draw axes
		mAxes.render();

	//6. Calibrate Coordinates
	_calibrateCoord();
	
	if(mShowBBox) //draw BBox
	{
		if(mNeedCalculateBBox==true)
		{
			_setupBBox();
			mNeedCalculateBBox = false;
		}
		mBBox.render();
	}
}

void BaseObject::render()
{
	float globalMatrix[16];
	glGetFloatv( GL_MODELVIEW_MATRIX , globalMatrix );
	_transformation();
	
	if(mNeedCalculateNormal) // check if need to re-calculate normal
		_calculateNormal();
	
	if(mShow) // draw itself
	{
		if(mRenderProfileName=="GLOBAL_PROFILE")
		{
			glPushAttrib(GL_LIGHTING_BIT);
			mEnableLighting ? glEnable(GL_LIGHTING) : glDisable(GL_LIGHTING);
			//mEnableColorTracking? glEnable(GL_COLOR_MATERIAL): glDisable(GL_COLOR_MATERIAL);
			drawEntity();
			glPopAttrib();
		}
	}
	_renderChildren(); // render the children if necessary
	
	glLoadMatrixf(globalMatrix);
}

void BaseObject::_renderChildren()
{
	list<BaseObject*>::iterator i;

	for(i=mChildrenList.begin();i!=mChildrenList.end();i++)
		(*i)->render();
}

Ball::Ball(string name) : BaseObject(name), mMaterial("MAT")
{
	mRadius = 10.0f;
};

void Ball::setMaterial(const Material& mat, unsigned int index,
		FACE_TYPE face)
{
	mMaterial = mat;
}

void Ball::setColor(const Colorf& color,unsigned int index, FACE_TYPE face )
{
	mColor = color;
}

void Ball::drawEntity()
{
	GLUquadricObj *quadric = NULL;
	glPolygonMode(GL_FRONT, GL_FILL);
	quadric = gluNewQuadric();

	//mEnableLighting ? mMaterial.apply(): glColor3fv(mColor.rgb);

	gluSphere(quadric,mRadius,32,32);
	glDisable(GL_COLOR_MATERIAL);
	gluDeleteQuadric(quadric);
}

XYPlane::XYPlane()
{
	mXSize = mYSize = 10;
	_updateVertBuf();
	_initTexAndNormalBuf();
}
	
XYPlane::XYPlane(float x, float y)
{
	mXSize = x; mYSize = y;
	_updateVertBuf();
	_initTexAndNormalBuf();
}
	
void XYPlane::setSize(float x, float y)
{
	mXSize = x; mYSize = y;
	_updateVertBuf();
}

void XYPlane::_updateVertBuf()
{
	for(int i=0;i<4;i++)
		mVertBuf[i].z = 0;

	mVertBuf[0].x = -mXSize/2; mVertBuf[0].y = mYSize/2;
	mVertBuf[1].x = -mXSize/2; mVertBuf[1].y = -mYSize/2;
	mVertBuf[2].x = mXSize/2; mVertBuf[2].y = -mYSize/2;
	mVertBuf[3].x = mXSize/2; mVertBuf[3].y = mYSize/2;
}

void XYPlane::_initTexAndNormalBuf()
{
	mTexCoordBuf[0] = Vector2(0, 1);
	mTexCoordBuf[1] = Vector2(0, 0);
	mTexCoordBuf[2] = Vector2(1, 0);
	mTexCoordBuf[3] = Vector2(1, 1);

	mNormal = Vector3::UNIT_Z;
}

void XYPlane::drawEntity()
{
	int i;
	glBegin(GL_QUADS);
	
	glNormal3fv(mNormal.val);
	for(i=0;i<4;i++)
	{
		glTexCoord2f(mTexCoordBuf[i].x, mTexCoordBuf[i].y);
		glVertex3fv(mVertBuf[i].val);
	}
	glEnd();
}

////////////////////////////////////////////////////////////////////////////////////////////////
//
// Cube Class Begin
//
///////////////////////////////////////////////////////////////////////////////////////////////
Cube::Cube(const std::string& name, float size) : BaseObject(name), m_fSize(size)
{
	_initTexNormalBuf();
	_initFace();
	_updateVertBuf();
}
	
Cube::~Cube()
{}

void Cube::_initTexNormalBuf()
{
	mNormalBuf[0] = Vector3::UNIT_X;
	mNormalBuf[1] = Vector3::NEGATIVE_UNIT_X;

	mNormalBuf[2] = Vector3::UNIT_Y;
	mNormalBuf[3] = Vector3::NEGATIVE_UNIT_Y;

	mNormalBuf[4] = Vector3::UNIT_Z;
	mNormalBuf[5] = Vector3::NEGATIVE_UNIT_Z;

	mTexCoordBuf[0] = Vector2(0, 1);
	mTexCoordBuf[1] = Vector2(0, 0);
	mTexCoordBuf[2] = Vector2(1, 0);
	mTexCoordBuf[3] = Vector2(1, 1);
}

void Cube::_initFace()
{
	int i, f;

	// resize vector
	for(i=0;i<6;i++)
	{
		mFace[i].mVert_idx.resize(4);
		mFace[i].mNormal_idx.resize(4);
		mFace[i].mTexCoord_idx.resize(4);
	}

	for(f=0;f<6;f++)
	{
		for(i=0;i<4;i++)
		{
			mFace[f].mNormal_idx[i] = f;
			mFace[f].mTexCoord_idx[i] = i;
		}
	}

	// POS X
	mFace[0].mVert_idx[0] = 1; mFace[0].mVert_idx[1] = 2; mFace[0].mVert_idx[2] = 6; mFace[0].mVert_idx[3] = 5;
	
	// NEG X
	mFace[1].mVert_idx[0] = 4; mFace[1].mVert_idx[1] = 7; mFace[1].mVert_idx[2] = 3; mFace[1].mVert_idx[3] = 0;
	
	// POS Y
	mFace[2].mVert_idx[0] = 4; mFace[2].mVert_idx[1] = 0; mFace[2].mVert_idx[2] = 1; mFace[2].mVert_idx[3] = 5;
	
	// NEG Y
	mFace[3].mVert_idx[0] = 3; mFace[3].mVert_idx[1] = 7; mFace[3].mVert_idx[2] = 6; mFace[3].mVert_idx[3] = 2;
	
	// POS Z
	mFace[4].mVert_idx[0] = 0; mFace[4].mVert_idx[1] = 3; mFace[4].mVert_idx[2] = 2; mFace[4].mVert_idx[3] = 1;

	// NEG Z
	mFace[5].mVert_idx[0] = 5; mFace[5].mVert_idx[1] = 6; mFace[5].mVert_idx[2] = 7; mFace[5].mVert_idx[3] = 4;
}

void Cube::setMaterial(const Material &mat, unsigned int index, FACE_TYPE face)
{
	if(index < 0 || index > 5)
		return;
	mFrontMaterial[index] = mat;
}

void Cube::drawEntity()
{
	//glPushAttrib(GL_ENABLE_BIT | GL_LIGHTING_BIT);
	vector<unsigned int>::iterator vit;
	float u, v;
	int f,i;

	glPushAttrib(GL_LIGHTING_BIT | GL_POLYGON_BIT);
	glDisable(GL_LIGHTING);
	glPolygonMode( GL_FRONT,GL_FILL);

	glBegin(GL_QUADS);
	for(f=0;f<6;f++)
	{
		//mEnableLighting? mFrontMaterial[f].apply() : glColor3fv(mColor[f].rgb);
		for(i =0;i<4;i++)
		{
			glNormal3fv(mNormalBuf[f].val);
			u = mTexCoordBuf[mFace[f].mTexCoord_idx[i]].x;
			v = mTexCoordBuf[mFace[f].mTexCoord_idx[i]].y;
			glTexCoord2f(mTexCoordBuf[mFace[f].mTexCoord_idx[i]].x, mTexCoordBuf[mFace[f].mTexCoord_idx[i]].y);
			glVertex3fv(mVertBuf[mFace[f].mVert_idx[i]].val);
		}
	}
	glEnd();
	glPopAttrib();
}

void Cube::_updateVertBuf()
{
	mVertBuf[0].x = -m_fSize/2;
	mVertBuf[0].y = m_fSize/2;
	mVertBuf[0].z = m_fSize/2;

	mVertBuf[1].x = m_fSize/2;
	mVertBuf[1].y = m_fSize/2;
	mVertBuf[1].z = m_fSize/2;

	mVertBuf[2].x = m_fSize/2;
	mVertBuf[2].y =  -m_fSize/2;
	mVertBuf[2].z = m_fSize/2;

	mVertBuf[3].x =  -m_fSize/2;
	mVertBuf[3].y =  -m_fSize/2;
	mVertBuf[3].z = m_fSize/2;

	mVertBuf[4].x =  -m_fSize/2;
	mVertBuf[4].y = m_fSize/2;
	mVertBuf[4].z =  -m_fSize/2;

	mVertBuf[5].x = m_fSize/2;
	mVertBuf[5].y = m_fSize/2;
	mVertBuf[5].z =  -m_fSize/2;

	mVertBuf[6].x = m_fSize/2;
	mVertBuf[6].y =  -m_fSize/2;
	mVertBuf[6].z =  -m_fSize/2;

	mVertBuf[7].x =  -m_fSize/2;
	mVertBuf[7].y =  -m_fSize/2;
	mVertBuf[7].z =  -m_fSize/2;
}



BBox::BBox()
{
	mSize = Vector3::ZERO;
	mPosition = Vector3::ZERO;
}

void BBox::render()
{
	glPushAttrib(GL_LIGHTING_BIT);
	glDisable(GL_LIGHTING);
	
	glLineStipple(4, 0xAAAA);
	glEnable(GL_LINE_STIPPLE);
	glColor3ub(0xFF, 0x0, 0x0);
	glBegin(GL_LINE_LOOP);
	glVertex3fv(mVertBuf[0].val);
	glVertex3fv(mVertBuf[1].val);
	glVertex3fv(mVertBuf[2].val);
	glVertex3fv(mVertBuf[3].val);
	glEnd();

	glBegin(GL_LINE_LOOP);
	glVertex3fv(mVertBuf[4].val);
	glVertex3fv(mVertBuf[5].val);
	glVertex3fv(mVertBuf[6].val);
	glVertex3fv(mVertBuf[7].val);
	glEnd();

	glBegin(GL_LINES);
	glVertex3fv(mVertBuf[0].val);
	glVertex3fv(mVertBuf[4].val);
	glVertex3fv(mVertBuf[1].val);
	glVertex3fv(mVertBuf[5].val);
	glVertex3fv(mVertBuf[2].val);
	glVertex3fv(mVertBuf[6].val);
	glVertex3fv(mVertBuf[3].val);
	glVertex3fv(mVertBuf[7].val);
	glEnd();

	glDisable(GL_LINE_STIPPLE);
/*
	glBegin(GL_LINES);
	glVertex3f(mPosition.x + mSize.x/2, mPosition.y, mPosition.z);
	glVertex3f(mPosition.x, mPosition.y, mPosition.z);

	glColor3ub(0x0, 0xFF, 0x0);
	glVertex3f(mPosition.x , mPosition.y + mSize.y/2, mPosition.z);
	glVertex3f(mPosition.x, mPosition.y, mPosition.z);

	glColor3ub(0x0, 0x0, 0xFF);
	glVertex3f(mPosition.x , mPosition.y, mPosition.z + mSize.z/2);
	glVertex3f(mPosition.x, mPosition.y, mPosition.z);
	glEnd();
*/
	glPopAttrib();
}

void BBox::setPos(const Vector3& pos)
{
	mPosition = pos;
	_reCalculateCoord();
}

void BBox::setSize(float x, float y, float z)
{
	mSize.x = x;
	mSize.y = y;
	mSize.z = z;
	_reCalculateCoord();
}

void BBox::_reCalculateCoord(void)
{
	mVertBuf[0].x = mPosition.x - mSize.x/2;
	mVertBuf[0].y = mPosition.y + mSize.y/2;
	mVertBuf[0].z = mPosition.z + mSize.z/2;

	mVertBuf[1].x = mPosition.x + mSize.x/2;
	mVertBuf[1].y = mPosition.y + mSize.y/2;
	mVertBuf[1].z = mPosition.z + mSize.z/2;

	mVertBuf[2].x = mPosition.x + mSize.x/2;
	mVertBuf[2].y = mPosition.y - mSize.y/2;
	mVertBuf[2].z = mPosition.z + mSize.z/2;

	mVertBuf[3].x = mPosition.x - mSize.x/2;
	mVertBuf[3].y = mPosition.y - mSize.y/2;
	mVertBuf[3].z = mPosition.z + mSize.z/2;

	mVertBuf[4].x = mPosition.x - mSize.x/2;
	mVertBuf[4].y = mPosition.y + mSize.y/2;
	mVertBuf[4].z = mPosition.z - mSize.z/2;

	mVertBuf[5].x = mPosition.x + mSize.x/2;
	mVertBuf[5].y = mPosition.y + mSize.y/2;
	mVertBuf[5].z = mPosition.z - mSize.z/2;

	mVertBuf[6].x = mPosition.x + mSize.x/2;
	mVertBuf[6].y = mPosition.y - mSize.y/2;
	mVertBuf[6].z = mPosition.z - mSize.z/2;

	mVertBuf[7].x = mPosition.x - mSize.x/2;
	mVertBuf[7].y = mPosition.y - mSize.y/2;
	mVertBuf[7].z = mPosition.z - mSize.z/2;
}

Grid::Grid(float w, float h, int seg_w, int seg_h)
{
	mVertBuf = NULL;
	mNormalBuf = NULL;
	resetSize(w, h, seg_w, seg_h);

	mColor.r = 0.5f;
	mColor.g = 1.0f;
	mColor.b = 0.5f;
}

void Grid::resetSize(float w, float h, int seg_w, int seg_h)
{
	delete [] mVertBuf;
	delete [] mNormalBuf;

	int i, j, idx;
	float step_x, step_y;

	mHeight = h;
	mWidth = w;
	mSegH = seg_h;
	mSegW = seg_w;
	mVertBuf = new Vector3[(seg_h+1)*(seg_w+1)];
	mNormalBuf = new Vector3[(seg_h+1)*(seg_w+1)];

	if(mSegW==0)
		step_x = 0;
	else
		step_x = mWidth/mSegW;

	if(mSegH==0)
		step_y = 0;
	else
		step_y = mHeight/mSegH;

	for(j=0;j<mSegH+1;j++)
	{
		for(i=0;i<mSegW+1;i++)
		{
			idx =j*(mSegW+1) + i;
			mVertBuf[idx].x = step_x*i - mWidth/2.0f;
			mVertBuf[idx].y = step_y*j - mHeight/2.0f;
			mVertBuf[idx].z = 0.0f;
		}
	}

	mNeedCalculateBBox = true;
	mNeedCalculateNormal = true;
}

Grid::~Grid()
{
	delete [] mVertBuf;
	delete [] mNormalBuf;
}


void Grid::_calculateNormal(void)
{
	int i, j, idx;
	Vector3 normal;
	
	//Add by I-Chen 05/15/08
	if((mSegW==0)||(mSegH==0))
		return;

	mNormalBuf[0] = CalculateNormal(mVertBuf[1], mVertBuf[0], mVertBuf[mSegW+1]);
	mNormalBuf[mSegW] = CalculateNormal(mVertBuf[mSegW+1+mSegW], mVertBuf[mSegW], mVertBuf[mSegW-1]);
	idx = mSegH*(mSegW+1);
	mNormalBuf[idx] = CalculateNormal(mVertBuf[idx - (mSegW+1)], mVertBuf[idx], mVertBuf[idx+1]);
	idx = mSegH*(mSegW+1) + mSegW;
	mNormalBuf[idx] = CalculateNormal(mVertBuf[idx -1], mVertBuf[idx], mVertBuf[idx - (mSegW+1)]);


	for(i=1;i<mSegW;i++)
	{
		normal = Vector3::ZERO;
		normal += CalculateNormal(mVertBuf[i+1], mVertBuf[i], mVertBuf[i+mSegW+1]);
		normal += CalculateNormal(mVertBuf[i+mSegW+1], mVertBuf[i], mVertBuf[i-1]);
		normal.normalise();
		mNormalBuf[i] = normal;//Vector3::ZERO;//CalculateNormal(mVertBuf[i+1], mVertBuf[i], mVertBuf[i+mSegW+1]);
	}

	for(j=1;j<mSegH;j++)
	{
		normal = Vector3::ZERO;
		idx = j*(mSegW+1) + mSegW;
		normal += CalculateNormal(mVertBuf[idx+mSegW+1], mVertBuf[idx], mVertBuf[idx-1]);
		normal += CalculateNormal(mVertBuf[idx-1], mVertBuf[idx], mVertBuf[idx-(mSegW+1)]);
		normal.normalise();
		mNormalBuf[idx] = normal; 
	}

	for(i=mSegW-1;i>0;i--)
	{
		normal = Vector3::ZERO;
		idx = mSegH*(mSegW+1)+i;
		normal+= CalculateNormal(mVertBuf[idx -1], mVertBuf[idx], mVertBuf[idx - (mSegW+1)]);
		normal+= CalculateNormal(mVertBuf[idx - (mSegW+1)], mVertBuf[idx], mVertBuf[idx +1]);
		normal.normalise();
		mNormalBuf[idx] = normal;
	}

	for(j=mSegH-1; j>0; j--)
	{
		normal = Vector3::ZERO;
		idx = j*(mSegW+1);
		normal += CalculateNormal(mVertBuf[idx - (mSegW+1)], mVertBuf[idx], mVertBuf[idx+1]);
		normal += CalculateNormal(mVertBuf[idx+1], mVertBuf[idx], mVertBuf[idx + (mSegW+1)]);
		normal.normalise();
		mNormalBuf[idx] = normal;
	}

	for(j=1;j<mSegH;j++)
	{
		for(i=1;i<mSegW;i++)
		{
			normal = Vector3::ZERO;
			idx = j*(mSegW+1)+i;
			normal += CalculateNormal(mVertBuf[idx-1], mVertBuf[idx], mVertBuf[idx-(mSegW+1)]);
			normal += CalculateNormal(mVertBuf[idx-(mSegW+1)], mVertBuf[idx], mVertBuf[idx+1]);
			normal += CalculateNormal(mVertBuf[idx+(mSegW+1)], mVertBuf[idx], mVertBuf[idx-1]);
			normal += CalculateNormal(mVertBuf[idx+1], mVertBuf[idx], mVertBuf[idx+(mSegW+1)]);
			normal.normalise();
			mNormalBuf[idx] = normal;//Vector3::ZERO;//normal.normalise();
		}
	}
	mNeedCalculateNormal = false;
}

Vector3 Grid::getGridPosition(int x_idx, int y_idx) const
{
	return Vector3(mVertBuf[y_idx*(mSegW+1)+x_idx]);
}

void Grid::setZ(float val)
{
	int i, j, idx;
		
	for(j=0;j<mSegH+1;j++)
	{
		for(i=0;i<mSegW+1;i++)
		{
			idx =j*(mSegW+1) + i;
			mVertBuf[idx].z = val;
		}
	}
}


void Grid::setZ(int x_idx, int y_idx, float val)
{
	mVertBuf[y_idx*(mSegW+1)+x_idx].z = val;
	mNeedCalculateNormal = true;
	mNeedCalculateBBox = true;
}

void Grid::drawEntity()
{
	int i, j, idx;

	//mEnableLighting ? mMaterial.apply(): glColor3fv(mColor.rgb);

	glLineWidth(2.0f);

	//glPushAttrib();

	// render polygon mesh
	if(mRenderMode==RM_POLYGON)
		glPolygonMode( GL_FRONT_AND_BACK,GL_FILL);
	else if(mRenderMode==RM_WIRE)
		glPolygonMode( GL_FRONT_AND_BACK,GL_LINE);
	else
		assert(0);

	glBegin(GL_QUAD_STRIP);
	for(j=0;j<mSegH;j++)
	{
		for(i=0;i<mSegW;i++)
		{
			idx = j*(mSegW+1)+i;

			glNormal3fv(mNormalBuf[idx].val);
			glVertex3fv(mVertBuf[idx].val);

			glNormal3fv(mNormalBuf[idx+1].val);
			glVertex3fv(mVertBuf[idx+1].val);

			glNormal3fv(mNormalBuf[idx+(mSegW+1)].val);
			glVertex3fv(mVertBuf[idx+(mSegW+1)].val);

			glNormal3fv(mNormalBuf[idx+(mSegW+2)].val);
			glVertex3fv(mVertBuf[idx+(mSegW+2)].val);
		}
		
	}
	glEnd();
	glPopAttrib();

	if(mShowNormal)// Draw Normal
	{
		if(mNeedCalculateNormal)
			_calculateNormal();

		glPushAttrib(GL_LIGHTING_BIT);
		glDisable(GL_LIGHTING);
		glColor3f(0.5f, 0.5f, 1.0f);
		for(j=0;j<mSegH+1;j++)
		{
			for(i=0;i<mSegW+1;i++)
			{
				idx = j*(mSegW+1)+i;
				glBegin(GL_LINES);
				glVertex3fv(mVertBuf[idx].val);
				glVertex3f(mVertBuf[idx].x + mNormalBuf[idx].x, mVertBuf[idx].y + mNormalBuf[idx].y, mVertBuf[idx].z + mNormalBuf[idx].z);
				glEnd();
			}
		}
		glPopAttrib();
	}
}

void Grid::_calibrateCoord()
{
	//glTranslatef(-mCenter.x, -mCenter.y, -mCenter.z);
}

void Grid::_setupBBox()
{
	int i, j, k, idx;
	Vector3 max, min, size;
	max = Vector3::NEG_MAX;
	min = Vector3::POS_MAX;

	for(j=0;j<mSegH+1;j++)
	{
		for(i=0;i<mSegW+1;i++)
		{
			idx = j*(mSegW+1)+i;
			for(k=0;k<3;k++)
			{
				if(mVertBuf[idx].val[k]>max.val[k])
					max.val[k] = mVertBuf[idx].val[k];
				if(mVertBuf[idx].val[k]<min.val[k])
					min.val[k] = mVertBuf[idx].val[k];
			}
		}
	}

	for(i=0;i<3;i++)
	{
		size.val[i] = max.val[i] - min.val[i];
		if(size.val[i] <=0.0f)
			size.val[i] = 0.1f;
	}
	mBBox.setSize(size.x, size.y, size.z);
	mBBox.setPos(Vector3((max.x+min.x)/2, (max.y+min.y)/2, (max.z+min.z)/2));
}

///////////////////////////////////////////////////////////////////////
// LineStrip Class Begin
///////////////////////////////////////////////////////////////////////
LineStrip::LineStrip(int seg)
{
	mVertBuf = NULL;
	//mNormalBuf = NULL;
	resetSize(10, seg);
	mColor = Colorf::GREEN;
}

LineStrip::LineStrip(float length, int seg)
{
	mVertBuf = NULL;
	//mNormalBuf = NULL;
	resetSize(length, seg);
	mColor = Colorf::GREEN;
}

LineStrip::LineStrip(const Vector3 *vert_array, int num)
{
	int		i;

	mVertBuf = new Vector3 [num];

	mVertBuf[0] = vert_array[0];

	mLength = 0.0f;

	for (i = 1; i < num; i++)
	{
		mVertBuf[i] = vert_array[i];

		mLength += mVertBuf[i].distance (mVertBuf[i - 1]);
	}

	mSeg = num - 1;
	mColor = Colorf::GREEN;
	_setupBBox();
}

void LineStrip::resetSize(float length, int seg)
{
	if(seg<1)
		throw("Segment Amount is lower than acceptable value :(1)");

	int i;
	float step;
	
	delete [] mVertBuf;
	mVertBuf = new Vector3[seg+1];
	mSeg = seg;
	mLength = length;

	step = mLength/mSeg;

	for(i=0;i<seg+1;i++)
	{
		mVertBuf[i].x = step*i - mLength/2.0f;
		mVertBuf[i].y = mVertBuf[i].z = 0.0f;
	}
}

/*
LineStrip::LineStrip(LineStrip& line)
{
	// implement later
}*/

LineStrip::~LineStrip()
{
	delete [] mVertBuf;
	//delete [] mNormalBuf;
}

void LineStrip::setColor(float r, float g, float b)
{
	mColor.r = r;
	mColor.g = g;
	mColor.b = b;
}

void LineStrip::_setupBBox()
{
	int i, j;
	Vector3 max, min, size;
	max = Vector3::NEG_MAX;
	min = Vector3::POS_MAX;

	for(i=0;i<=mSeg;i++)
	{
		for(j=0;j<3;j++)
		{
			if(mVertBuf[i].val[j] > max.val[j])
				max.val[j] = mVertBuf[i].val[j];
			if(mVertBuf[i].val[j] < min.val[j])
				min.val[j] = mVertBuf[i].val[j];
		}
	}

	for(i=0;i<3;i++)
	{
		size.val[i] = max.val[i] - min.val[i];
		if(size.val[i] <=0.0f)
			size.val[i] = 0.1f;
	}
	mBBox.setSize(size.x, size.y, size.z);
	mBBox.setPos(Vector3((max.x+min.x)/2, (max.y+min.y)/2, (max.z+min.z)/2));
	mNeedCalculateBBox = false;
}

void LineStrip::_calibrateCoord ()
{
	Vector3 pos;
	pos = mBBox.getPosition ();

	glTranslatef (-pos.x, -pos.y, -pos.z);
}
/*
void LineStrip::_calculateNormal()
{
	delete [] mNormalBuf;
	mNormalBuf = new Vector3[mSeg+1];

	Vector2 v1, v2, v1_p, v2_p;
	int i;

	for(i=1;i<mSeg;i++)
	{
		v1.x = 1.0f;
		v1.y = mVertBuf[i].z - mVertBuf[i-1].z;
		v2.x = 1.0f;
		v2.y = mVertBuf[i+1].z - mVertBuf[i].z;
		
		v1_p = v1.perpendicular();
		v2_p = v2.perpendicular();
		v1_p.normalise();
		v2_p.normalise();
		v1 = v1_p + v2_p;
		v1.normalise();
		mNormalBuf[i].x = v1.x;
		mNormalBuf[i].y = 0.0f;
		mNormalBuf[i].z = v1.y;
	}

	v1.x = 1.0f;
	v1.y = mVertBuf[1].z - mVertBuf[0].z;
	v1_p = v1.perpendicular();
	v1_p.normalise();
	mNormalBuf[0].x = v1_p.x;
	mNormalBuf[0].y = 0.0f;
	mNormalBuf[0].z = v1_p.y;

	v1.x = 1.0f;
	v1.y = mVertBuf[mSeg].z - mVertBuf[mSeg-1].z;
	v1_p = v1.perpendicular();
	v1_p.normalise();
	mNormalBuf[mSeg].x = v1_p.x;
	mNormalBuf[mSeg].y = 0.0f;
	mNormalBuf[mSeg].z = v1_p.y;
}
*/
const Vector3 LineStrip::getGridPosition(int idx) const
{
	//if(idx<0||idx>mSeg)
	//	Log::GetSingleton().dump("index out of range", __FILE__, __LINE__);
	return mVertBuf[idx];
}

void LineStrip::setXYZ(int idx, float x, float y, float z)
{
	mVertBuf[idx].x = x;
	mVertBuf[idx].y = y;
	mVertBuf[idx].z = z;
}

void LineStrip::setX(int idx, float val)
{
	mVertBuf[idx].x = val;
}

void LineStrip::setY(int idx, float val)
{
	mVertBuf[idx].y = val;
}

void LineStrip::setZ(int idx, float val)
{
	mVertBuf[idx].z = val;
}

void LineStrip::drawEntity()
{
	int i;

	glPushAttrib(GL_LIGHTING_BIT);
	glDisable(GL_LIGHTING);

	glColor4fv(mColor.rgba);
	glBegin(GL_LINE_STRIP);
	
	for(i=0;i<mSeg+1;i++)
		glVertex3fv(mVertBuf[i].val);
	
	glEnd();

	glPopAttrib();
}


const char BaseObject::ENTITY_BIT = 0x1;
const char BaseObject::BBOX_BIT = 0x2;
const char BaseObject::AXES_BIT = 0x4;

}